import numpy as np
import matplotlib.pyplot as plt
from math import cos, sin, pi, sqrt
import data_process
from flightData.data.twenty_eight import get_xy_uav, get_altitude, get_attitude, get_PTZ_attitude


# 相机坐标系->大地坐标系
def get_xy(time):
    list_ptz_xy = get_PTZ_xy(time)
    x_uav, y_uav = get_xy_uav(time)
    xy_uav = np.array([[y_uav], [x_uav]])
    # matrix_to_geo = c_bc(time)
    matrix_to_geo = np.dot(c_nb(time), c_bc(time))

    matrix_xy_ptz = list_ptz_xy

    deviation = np.dot(matrix_to_geo, matrix_xy_ptz)

    # print("第%d次相对位置：\n" % time, deviation)
    deviation[0][0] = deviation[0][0] / (111000 * cos(xy_uav[1][0] * pi / 180))
    deviation[1][0] = deviation[1][0] / 111000

    xy_ans = xy_uav + deviation[0:2, :]
    print("第%d次最终位置：\n" % time, xy_ans)

    return xy_ans[0][0], xy_ans[1][0]


# 目标在云台(相机)坐标系坐标
def get_PTZ_xy(time):
    x_uav, y_uav = get_xy_uav(time)
    xy_uav = np.array([[x_uav], [y_uav]])
    uv_arr = data_process.load_image_uv(time)
    matrix_left = np.array([[250, 0, 0], [0, 250, 0], [0, 0, 1]])

    u, v = uv_arr[0], uv_arr[1]
    # [x, y, 1]
    image_axis = np.array([[u], [v], [1]])
    # △zpu
    altitude = get_altitude(time)

    matrix_cam = np.dot(matrix_left, image_axis) * altitude
    # print("云台坐标系下：", matrix_cam)

    return matrix_cam


def c_nb(time):
    # θ     φ    ψ
    # theta, phi, psi = get_attitude(time)[0], get_attitude(time)[2], get_attitude(time)[1]
    theta, phi, psi = 0, get_attitude(time)[1], 0

    theta, phi, psi = theta * pi / 180, phi * pi / 180, psi * pi / 180

    # matrix_1 = np.array([[cos(psi), 0, sin(psi)], [0, 1, 0], [-sin(psi), 0, cos(psi)]])
    # matrix_2 = np.array([[cos(theta), sin(theta), 0], [-sin(theta), cos(theta), 0], [0, 0, 1]])
    # matrix_3 = np.array([[1, 0, 0], [0, cos(phi), -sin(phi)], [0, sin(phi), cos(phi)]])

    matrix_1 = np.array([[cos(theta), 0, sin(theta)], [0, 1, 0], [-sin(theta), 0, cos(theta)]])
    matrix_2 = np.array([[cos(psi), sin(psi), 0], [-sin(psi), cos(psi), 0], [0, 0, 1]])
    matrix_3 = np.array([[1, 0, 0], [0, cos(phi), -sin(phi)], [0, sin(phi), cos(phi)]])

    return np.dot(np.dot(matrix_1, matrix_2), matrix_3)


def c_bc(time):
    # α： Alpha
    # β:  Beta
    alpha, beta = get_PTZ_attitude(time)[1], get_PTZ_attitude(time)[0] + 90
    alpha, beta = alpha * pi / 180, beta * pi / 180
    matrix_1 = np.array([[cos(alpha), sin(alpha), 0], [-sin(alpha), cos(alpha), 0], [0, 0, 1]])
    matrix_2 = np.array([[cos(beta), 0, sin(beta)], [0, 1, 0], [-sin(beta), 0, cos(beta)]])

    return np.dot(matrix_1, matrix_2)


def one_point_check(times):
    plt.figure()
    xy_ans = get_xy(times)
    plt.plot([xy_ans[0]], [xy_ans[1]], 'go')
    x_uav, y_uav = get_xy_uav(times)
    plt.plot([118.79007], [31.93906], 'rx')  # 基准点
    plt.show()


def plot_in_geo():
    uav = []
    target = []
    for times in range(29):
        if times in [1, 2, 21, 27, 28]: # 移除误差较大的点
            continue
        x_uav, y_uav = get_xy_uav(times)
        uav.append([y_uav, x_uav])
        x, y = get_xy(times)
        target.append([x, y])
    plt.figure()
    plt.xlabel("Longitude")
    plt.ylabel("Latitude")
    plt.grid()
    for i in range(len(uav)):
        point1, = plt.plot([uav[i][0]], [uav[i][1]], 'bo')
        point2, = plt.plot([target[i][0]], [target[i][1]], 'rx')
    arr = [[118.79012515, 31.93907387], [118.7901237, 31.93907011], [118.79011685, 31.93908552],
           [118.7901094, 31.93910106], [118.79010399, 31.9390949], [118.79010002, 31.93905846]
           ]
    for i in range(len(arr)):
        point3, = plt.plot([arr[i][0]-0.00003], [arr[i][1]-0.0000178], 'gx')
    plt.legend(handles=[point2, point1], labels=['target position', 'UAV position'])
    plt.show()


def plot_on_earth():
    target = []
    for times in range(29):
        if times in [1, 2, 21, 27, 28]:
            continue
        x, y = get_xy(times)
        target.append([x, y])
    plt.figure()
    plt.xlim((-4, 4))
    plt.ylim((-3, 3))
    plt.xlabel("y-deviation(m)")
    plt.ylabel("x-deviation(m)")
    plt.grid()
    for i in range(len(target)):
        x = target[i][0] - 118.7900870
        y = target[i][1] - 31.9390670
        x_dev = x * 111000
        y_dev = y * 111000 * cos(31.939 * pi / 180)
        point1, = plt.plot([x_dev], [y_dev], 'rx')
        print("第%d次偏移：" % i, sqrt(x_dev * x_dev + y_dev * y_dev))
    plt.legend(handles=[point1], labels=['target position'])
    plt.show()


if __name__ == "__main__":
    plot_in_geo()
    plot_on_earth()


